/**
 * @file chassis.c
 * @author vaine
 * @brief 控制麦轮底盘驱动板
 * @version 0.1
 * @date 2021-07-04
 * 
 * @copyright Copyright (c) 2021
 * 
 */

#include "chassis.h"
#include <math.h>
#include "oslib_uart.h"

static CHASSIS_PROTOCAL data_send;
static int8_t WheelTread_hf = 13; // cm
static int8_t WheelBase_hf = 13;  // cm

void set_speed_25ga(uint8_t id, int8_t speed)
{
    data_send.head = CHASSIS_HEAD;
    data_send.len = sizeof(CHASSIS_PROTOCAL);
    data_send.motor_id = id;
    data_send.board_id = CHASSIS_ID;
    data_send.speed_set = speed;
    data_send.sign_speed = speed >> 7;
    data_send.check_sum = CHASSIS_END;

    uint8_t *temp_send = (uint8_t *)&data_send;
    OSLIB_Uart_SendData(&huart2, temp_send, sizeof(CHASSIS_PROTOCAL));
    OSLIB_Uart_SendData(&huart2, temp_send, sizeof(CHASSIS_PROTOCAL));
}

void STOP()
{
    set_speed_25ga(1, 0);
    set_speed_25ga(2, 0);
    set_speed_25ga(3, 0);
    set_speed_25ga(4, 0);
}

void Move_FB(int8_t speed)
{
    set_speed_25ga(1, speed);
    set_speed_25ga(2, speed);
    set_speed_25ga(3, speed);
    set_speed_25ga(4, speed);
}

void Move_CW(int8_t speed)
{
    set_speed_25ga(1, -speed);
    set_speed_25ga(2, speed);
    set_speed_25ga(3, -speed);
    set_speed_25ga(4, speed);
}

void Move_LF(int8_t speed)
{
    set_speed_25ga(1, -speed);
    set_speed_25ga(2, speed);
    set_speed_25ga(3, speed);
    set_speed_25ga(4, -speed);
}

void Turn_FL(int8_t radius, int8_t vel)
{
    if (radius == 0)
    {
        // 当旋转半径为0时, 原地旋转
        Move_CW(vel);
    }
    else if (radius == 127 || radius == -128)
    {
        // 当旋转半径为127或-128时, 视为无穷大, 跑直线
        Move_FB(vel);
    }
    else
    {
        int8_t speed_13 = 0, speed_24 = 0;
        if (radius > 0)
        {
            speed_24 = vel;
            if (radius > WheelTread_hf + WheelBase_hf)
            {
                speed_13 = vel - (WheelTread_hf + WheelBase_hf) * vel / radius;
            }
            else
            {
                // speed_13 = (WheelTread_hf + WheelBase_hf) * vel / radius - vel;
                speed_13 = vel - (WheelTread_hf + WheelBase_hf) * vel / radius;
                if (speed_13 < -vel)
                {
                    speed_13 = -vel;
                }
            }
        }
        else
        {
            radius = -radius;
            speed_13 = vel;
            if (radius > WheelTread_hf + WheelBase_hf)
            {
                speed_24 = vel - (WheelTread_hf + WheelBase_hf) * vel / radius;
            }
            else
            {
                // speed_24 = (WheelTread_hf + WheelBase_hf) * vel / radius - vel;
                speed_24 = vel - (WheelTread_hf + WheelBase_hf) * vel / radius;
                if(speed_24 < -vel){
                    speed_24 = -vel;
                }
            }
        }
        set_speed_25ga(1, speed_13);
        set_speed_25ga(2, speed_24);
        set_speed_25ga(3, speed_13);
        set_speed_25ga(4, speed_24);
        // uprintf("%d\r\n", speed_13);
        // uprintf("%d\r\n", speed_24);
    }
}
